#include "Serial.h"

int SCIA_Counter = 0;

#define CRC32_POLYNOMIAL 0xEDB88320L

/* --------------------------------------------------------------------------
Calculate a CRC value to be used by CRC calculation functions.
-------------------------------------------------------------------------- */
unsigned int CRC32Value(int i)
{
int j;
unsigned int ulCRC;
ulCRC = i;
for ( j = 8 ; j > 0; j-- )
{
if ( ulCRC & 1 )
ulCRC = ( ulCRC >> 1 ) ^ CRC32_POLYNOMIAL;
else
ulCRC >>= 1;
}
return ulCRC;
}
/* --------------------------------------------------------------------------
Calculates the CRC-32 of a block of data all at once
-------------------------------------------------------------------------- */
U32_Byte CalculateBlockCRC32(unsigned int ulCount, /* Number of bytes in the data block */Uint32 *ucBuffer ) /* Data block */
{
unsigned int ulTemp1; 
unsigned int ulTemp2;
unsigned int ulCRC= 0; 
U32_Byte ulCRC2;
while ( ulCount-- != 0 )
{
ulTemp1 = ( ulCRC >> 8 ) & 0x00FFFFFFL;
ulTemp2 = CRC32Value( ((int) ulCRC ^ *ucBuffer++ ) & 0xff ); 
ulCRC = ulTemp1 ^ ulTemp2;
}
ulCRC2.BYTE.Byte_0 = ulCRC && 0xff;
ulCRC2.BYTE.Byte_1 = (ulCRC>>8) && 0xff;
ulCRC2.BYTE.Byte_2 = (ulCRC>>16) && 0xff;
ulCRC2.BYTE.Byte_3 = (ulCRC>>24) && 0xff;

return( ulCRC2 );
}



Uint16 crc16_IBM(unsigned char *data, Uint32 length)
{
    unsigned char  i;
    Uint16 crc = 0;        // Initial value
    while(length--)
    {
        crc ^= *data++;        // crc ^= *data; data++;
        for (i = 0; i < 8; ++i)
        {
            if (crc & 1)
                crc = (crc >> 1) ^ 0xA001;        // 0xA001 = reverse 0x8005
            else
                crc = (crc >> 1);
        }
    }
    return crc;
}


void SCIA_TO_FLC(struct Message m, unsigned char * data_buf)
{

    //CRC
    // static unsigned char buf32_crc[168];//CRC缓存数组
    static unsigned char SCIA_Txdata[168];
    U32_Byte send_crc;//CRC最终结果
    unsigned int CRC_count = 164;//CRC缓存遍历使用
    //结构体实例化
    U32_Byte U32_DATA;
    i32_Byte i32_DATA;
    i16_Byte i16_DATA;
    U16_Byte U16_DATA;
    U64_Byte U64_DATA;
    doub_Byte doub_DATA;
    sing_Byte sing_DATA;
    Uint32 buf32_crc[164];


    // Uint8 SCIA_Txdata[168]; 
    //帧头
    SCIA_Txdata[0] = 0xAA;  //Sync
    SCIA_Txdata[1] = 0x44;  //Sync
    SCIA_Txdata[2] = 0x12;  //Sync
    SCIA_Txdata[3] = 0x1C;  //Header length
    // time = time?

    U16_DATA.U16 = (Uint16)(m.MessageID);  //Message ID
    SCIA_Txdata[4] = U16_DATA.BYTE.Byte_0;
    SCIA_Txdata[5] = U16_DATA.BYTE.Byte_1;
    
    SCIA_Txdata[6] = 0x00;  //Message type
    SCIA_Txdata[7] = 0x00;  //Reserved

    U16_DATA.U16 = (Uint16)(m.MessageLength);  //Message ID
    SCIA_Txdata[8] = U16_DATA.BYTE.Byte_0;
    SCIA_Txdata[9] = U16_DATA.BYTE.Byte_1;
    U16_DATA.U16 = (Uint16)(0);  //Reserved
    SCIA_Txdata[10] = U16_DATA.BYTE.Byte_0;
    SCIA_Txdata[11] = U16_DATA.BYTE.Byte_1;
    SCIA_Txdata[12] = 0x00;  //Reserved
    SCIA_Txdata[13] = 0x00;  //Reserved
    U16_DATA.U16 = (Uint16)(0);  //Reserved
    SCIA_Txdata[14] = U16_DATA.BYTE.Byte_0;
    SCIA_Txdata[15] = U16_DATA.BYTE.Byte_1;
    U32_DATA.U32 = (Uint32)(0);  //Reserved
    SCIA_Txdata[16] = U32_DATA.BYTE.Byte_0;
    SCIA_Txdata[17] = U32_DATA.BYTE.Byte_1;
    SCIA_Txdata[18] = U32_DATA.BYTE.Byte_2;
    SCIA_Txdata[19] = U32_DATA.BYTE.Byte_3;
    U32_DATA.U32 = (Uint32)(0);  //Reserved
    SCIA_Txdata[20] = U32_DATA.BYTE.Byte_0;
    SCIA_Txdata[21] = U32_DATA.BYTE.Byte_1;
    SCIA_Txdata[22] = U32_DATA.BYTE.Byte_2;
    SCIA_Txdata[23] = U32_DATA.BYTE.Byte_3;
    U16_DATA.U16 = (Uint16)(0);  //Reserved
    SCIA_Txdata[24] = U16_DATA.BYTE.Byte_0;
    SCIA_Txdata[25] = U16_DATA.BYTE.Byte_1;
    U16_DATA.U16 = (Uint16)(0);  //Reserved
    SCIA_Txdata[26] = U16_DATA.BYTE.Byte_0;
    SCIA_Txdata[27] = U16_DATA.BYTE.Byte_1;

    // 帧内容
    U64_DATA.U64 = (Uint64)(m.DeviceNumber);  //Device number
    SCIA_Txdata[28] = U64_DATA.BYTE.Byte_0;
    SCIA_Txdata[29] = U64_DATA.BYTE.Byte_1;
    SCIA_Txdata[30] = U64_DATA.BYTE.Byte_2;
    SCIA_Txdata[31] = U64_DATA.BYTE.Byte_3;
    SCIA_Txdata[32] = U64_DATA.BYTE.Byte_4;
    SCIA_Txdata[33] = U64_DATA.BYTE.Byte_5;

    SCIA_Txdata[34] = (Uint8)(m.role); //设备角色，0：锚节点，1：侦察节点
    SCIA_Txdata[35] = (Uint8)(m.Year); //UTC-年
    SCIA_Txdata[36] = (Uint8)(m.Month); //UTC-月
    SCIA_Txdata[37] = (Uint8)(m.Day); //UTC-日
    SCIA_Txdata[38] = (Uint8)(m.Hour); //UTC-时
    SCIA_Txdata[39] = (Uint8)(m.Minute); //UTC-分
    SCIA_Txdata[40] = (Uint8)(m.Second); //UTC-秒
    U16_DATA.U16 = (Uint16)(m.Millisecond);  //UTC-毫秒
    SCIA_Txdata[41] = U16_DATA.BYTE.Byte_0;
    SCIA_Txdata[42] = U16_DATA.BYTE.Byte_1;

    U16_DATA.U16 = (Uint16)(m.FrameCnt);  //帧计数
    SCIA_Txdata[43] = U16_DATA.BYTE.Byte_0;
    SCIA_Txdata[44] = U16_DATA.BYTE.Byte_1;

    SCIA_Txdata[45] = (Uint8)(m.GNSS_status);  //0：未定位，1：单点，2：伪距差分，3：RTKINT，4RTKfloat
    SCIA_Txdata[46] = (Uint8)(m.UWB_status);  //
    
    doub_DATA.doub = (double)(m.lx);    //纬度，单位：度
    SCIA_Txdata[47] = doub_DATA.BYTE.Byte_0;
    SCIA_Txdata[48] = doub_DATA.BYTE.Byte_1;
    SCIA_Txdata[49] = doub_DATA.BYTE.Byte_2;
    SCIA_Txdata[50] = doub_DATA.BYTE.Byte_3;
    SCIA_Txdata[51] = doub_DATA.BYTE.Byte_4;
    SCIA_Txdata[52] = doub_DATA.BYTE.Byte_5;
    SCIA_Txdata[53] = doub_DATA.BYTE.Byte_6;
    SCIA_Txdata[54] = doub_DATA.BYTE.Byte_7;
    doub_DATA.doub = (double)(m.ly);    //经度，单位：度
    SCIA_Txdata[55] = doub_DATA.BYTE.Byte_0;
    SCIA_Txdata[56] = doub_DATA.BYTE.Byte_1;
    SCIA_Txdata[57] = doub_DATA.BYTE.Byte_2;
    SCIA_Txdata[58] = doub_DATA.BYTE.Byte_3;
    SCIA_Txdata[59] = doub_DATA.BYTE.Byte_4;
    SCIA_Txdata[60] = doub_DATA.BYTE.Byte_5;
    SCIA_Txdata[61] = doub_DATA.BYTE.Byte_6;
    SCIA_Txdata[62] = doub_DATA.BYTE.Byte_7;
    doub_DATA.doub = (double)(m.lz);    //海拔高度，单位：米
    SCIA_Txdata[63] = doub_DATA.BYTE.Byte_0;
    SCIA_Txdata[64] = doub_DATA.BYTE.Byte_1;
    SCIA_Txdata[65] = doub_DATA.BYTE.Byte_2;
    SCIA_Txdata[66] = doub_DATA.BYTE.Byte_3;
    SCIA_Txdata[67] = doub_DATA.BYTE.Byte_4;
    SCIA_Txdata[68] = doub_DATA.BYTE.Byte_5;
    SCIA_Txdata[69] = doub_DATA.BYTE.Byte_6;
    SCIA_Txdata[70] = doub_DATA.BYTE.Byte_7;
    sing_DATA.sing = (float)(m.HAcc); //卫星定位水平精度因子 HDOP 值
    SCIA_Txdata[71] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[72] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[73] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[74] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.HAcc); //卫星定位水平精度因子 VDOP 值
    SCIA_Txdata[75] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[76] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[77] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[78] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.x); //相对位置 X 轴坐标，m
    SCIA_Txdata[79] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[80] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[81] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[82] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.y); //相对位置 Y 轴坐标，m
    SCIA_Txdata[83] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[84] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[85] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[86] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.z); //相对位置 Z 轴坐标，m
    SCIA_Txdata[87] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[88] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[89] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[90] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.deltaX); //UWB x 误差，m
    SCIA_Txdata[91] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[92] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[93] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[94] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.deltaY); //UWB y 误差，m
    SCIA_Txdata[95] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[96] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[97] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[98] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.deltaZ); //UWB z 误差，m
    SCIA_Txdata[99] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[100] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[101] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[102] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.vx); //X 轴速度，m/s
    SCIA_Txdata[103] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[104] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[105] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[106] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.vy); //Y 轴速度，m/s
    SCIA_Txdata[107] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[108] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[109] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[110] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.vz); //Z 轴速度，m/s
    SCIA_Txdata[111] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[112] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[113] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[114] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.SpeedGround); //卫星地面速度，单位：节
    SCIA_Txdata[115] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[116] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[117] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[118] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.Cog); //卫星地面航向，北起顺时针，单位：度
    SCIA_Txdata[119] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[120] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[121] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[122] = sing_DATA.BYTE.Byte_3;
    doub_DATA.doub = (double)(m.latitude_f);    //室内外融合后纬度，单位：度
    SCIA_Txdata[123] = doub_DATA.BYTE.Byte_0;
    SCIA_Txdata[124] = doub_DATA.BYTE.Byte_1;
    SCIA_Txdata[125] = doub_DATA.BYTE.Byte_2;
    SCIA_Txdata[126] = doub_DATA.BYTE.Byte_3;
    SCIA_Txdata[127] = doub_DATA.BYTE.Byte_4;
    SCIA_Txdata[128] = doub_DATA.BYTE.Byte_5;
    SCIA_Txdata[129] = doub_DATA.BYTE.Byte_6;
    SCIA_Txdata[130] = doub_DATA.BYTE.Byte_7;
    doub_DATA.doub = (double)(m.longitude_f);    //室内外融合后经度，单位：度
    SCIA_Txdata[131] = doub_DATA.BYTE.Byte_0;
    SCIA_Txdata[132] = doub_DATA.BYTE.Byte_1;
    SCIA_Txdata[133] = doub_DATA.BYTE.Byte_2;
    SCIA_Txdata[134] = doub_DATA.BYTE.Byte_3;
    SCIA_Txdata[135] = doub_DATA.BYTE.Byte_4;
    SCIA_Txdata[136] = doub_DATA.BYTE.Byte_5;
    SCIA_Txdata[137] = doub_DATA.BYTE.Byte_6;
    SCIA_Txdata[138] = doub_DATA.BYTE.Byte_7;
    doub_DATA.doub = (double)(m.altitude_f);    //室内外融合后海拔高度，单位：米
    SCIA_Txdata[139] = doub_DATA.BYTE.Byte_0;
    SCIA_Txdata[140] = doub_DATA.BYTE.Byte_1;
    SCIA_Txdata[141] = doub_DATA.BYTE.Byte_2;
    SCIA_Txdata[142] = doub_DATA.BYTE.Byte_3;
    SCIA_Txdata[143] = doub_DATA.BYTE.Byte_4;
    SCIA_Txdata[144] = doub_DATA.BYTE.Byte_5;
    SCIA_Txdata[145] = doub_DATA.BYTE.Byte_6;
    SCIA_Txdata[146] = doub_DATA.BYTE.Byte_7;
    sing_DATA.sing = (float)(m.velocityX_f); //室内外融合后X 轴速度，m/s
    SCIA_Txdata[147] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[148] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[149] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[150] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.velocityY_f); //室内外融合后Y 轴速度，m/s
    SCIA_Txdata[151] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[152] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[153] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[154] = sing_DATA.BYTE.Byte_3;
    sing_DATA.sing = (float)(m.velocityZ_f); //室内外融合后Z 轴速度，m/s
    SCIA_Txdata[155] = sing_DATA.BYTE.Byte_0;
    SCIA_Txdata[156] = sing_DATA.BYTE.Byte_1;
    SCIA_Txdata[157] = sing_DATA.BYTE.Byte_2;
    SCIA_Txdata[158] = sing_DATA.BYTE.Byte_3;

    SCIA_Txdata[159] = (Uint8)(m.satelliteNumber);    //卫星数量

    U32_DATA.U32 = (Uint32)(0); //Reserved
    SCIA_Txdata[160] = U32_DATA.BYTE.Byte_0;
    SCIA_Txdata[161] = U32_DATA.BYTE.Byte_1;
    SCIA_Txdata[162] = U32_DATA.BYTE.Byte_2;
    SCIA_Txdata[163] = U32_DATA.BYTE.Byte_3;

    // CRC校验和

    for(int i = 0; i < 164; i++)
    {
        buf32_crc[i] = SCIA_Txdata[i];
    }

    send_crc = CalculateBlockCRC32(CRC_count, buf32_crc);

    SCIA_Txdata[164] = send_crc.BYTE.Byte_0;
    SCIA_Txdata[165] = send_crc.BYTE.Byte_1;
    SCIA_Txdata[166] = send_crc.BYTE.Byte_2;
    SCIA_Txdata[167] = send_crc.BYTE.Byte_3;

    SCIA_Counter++;

    // SCIA_Tx_State.State = Start_Tx;
    // SCIA_Tx_State.Field_Number = One;
    // SciaRegs.SCIFFTX.bit.TXFFINTCLR = 1;//打开串口发送寄存器，调用发送中断函数
}

